#include "CollisionTimeFinder.h"

namespace coolpool
{
	CollisionTimeFinder::CollisionTimeFinder()
	{

	}

	const Collision * CollisionTimeFinder::findCollisionTimeWithDetectorStatic(const CollisionDetector & collision_detector, DynamicEntity * dynamic_entity, Entity * entity, double time_step,
			double lambda) throw (UnexpectedInputException)
	{
		dynamic_entity->onUpdate(time_step);

		//	Missing on purpose.
		// entity->onUpdate(time_step);

		Collision * col_init;
		if ((col_init = collision_detector.detect(dynamic_entity, entity)))
		{
			if (col_init->type == MOVINGAWAY)
			{
				// Reset object.
				dynamic_entity->reverse();
				// Not necessary to inform the collision detection engine.
				delete col_init;
				return NULL;
			}

			// Go back to the previous state.
			dynamic_entity->reverse();

			double start = 0.0;
			double end = time_step;
			double mid;
			double fine_step = time_step / BIN_SEARCH_ACCURACY;

			int counter = 0;
			Collision * col = NULL;
			while (end - start >= fine_step || !col)
			{
				if (col)
					delete col;
				// Infinit loop prevention.
				if (counter++ > MAX_STEPS_BIN_SEARCH)
				{
					col_init->time = mid;
					dynamic_entity->reverse();
					return col_init;
				}

				mid = (start + end) / 2;
				dynamic_entity->onUpdate(mid);
				if (!(col = collision_detector.detect(dynamic_entity, entity)))
				{
					start = mid + fine_step;
				}
				else
				{
					end = mid - fine_step;
				}
				// Reset the object.
				dynamic_entity->reverse();
			}

			col->time = mid;
			delete col_init;
			return col;
		}
		else
		{
			// Reset the object.
			dynamic_entity->reverse();
			return NULL;
		}
	}

	const Collision * CollisionTimeFinder::findCollisionTimeWithDetectorDynamic(const CollisionDetector & collision_detector, DynamicEntity * dynamic_entity1, DynamicEntity * dynamic_entity2,
			double time_step, double lambda) throw (UnexpectedInputException)
	{
		assert(dynamic_entity1 != dynamic_entity2);
		dynamic_entity1->onUpdate(time_step);
		dynamic_entity2->onUpdate(time_step);

		Collision * col_init;
		if ((col_init = collision_detector.detect(dynamic_entity1, dynamic_entity2)))
		{
			if (col_init->type == MOVINGAWAY)
			{
				// Reset the object.
				dynamic_entity1->reverse();
				dynamic_entity2->reverse();
				// Not necessary to inform the collision detection engine.
				delete col_init;
				return NULL;
			}

			// Reset the objects.
			dynamic_entity1->reverse();
			dynamic_entity2->reverse();

			double start = 0.0;
			double end = time_step;
			double mid;
			double fine_step = time_step / BIN_SEARCH_ACCURACY;

			int counter = 0;
			Collision * col = NULL;
			while (end - start > fine_step || !col)
			{
				if (col)
					delete col;
				// Infinit loop prevention.
				if (counter++ > MAX_STEPS_BIN_SEARCH)
				{
					// Reset object.
					dynamic_entity1->reverse();
					dynamic_entity2->reverse();

					col_init->time = mid;
					return col_init;
				}

				mid = (start + end) / 2;
				dynamic_entity1->onUpdate(mid);
				dynamic_entity2->onUpdate(mid);
				if (!(col = collision_detector.detect(dynamic_entity1, dynamic_entity2)))
				{
					start = mid + fine_step;
				}
				else
				{
					end = mid - fine_step;
				}
				dynamic_entity1->reverse();
				dynamic_entity2->reverse();
			}

			// Reset the object.
			dynamic_entity1->reverse();
			dynamic_entity2->reverse();
			col->time = mid;
			delete col_init;
			return col;

		}
		else
		{
			// Reset objects before return.
			dynamic_entity1->reverse();
			dynamic_entity2->reverse();
			return NULL;
		}
	}
}
